package project.robot.pathplanning;


import java.util.ArrayList;

import project.map.OccupancyGridMap;
import project.map.Position;
import project.common.Constants;


public class BfsPathPlanner extends PathPlanningSystem {
	
	public ArrayList<Integer> searchPathTo(Position currentPosition, Position goal, OccupancyGridMap map)
	{
		ArrayList<PathSearchNode> closedNodes = new ArrayList<PathSearchNode>();
		ArrayList<PathSearchNode> openNodes = new ArrayList<PathSearchNode>();
		ArrayList<PathSearchNode> childrenNodes = new ArrayList<PathSearchNode>();
		ArrayList<Integer> path = new ArrayList<Integer>();
				
		PathSearchNode currentNode = null;  
		openNodes.add(new PathSearchNode(currentPosition, null,0));

		boolean goalReached = false;

		while (openNodes.size() >0 && !goalReached)
		{
			currentNode = openNodes.get(0);

			openNodes.remove(currentNode);
			closedNodes.add(currentNode);

			if (!currentNode.position.equals(goal))
			{
				childrenNodes= new ArrayList<PathSearchNode>();
				
				if (map.isNorthFree(currentNode.position))
					childrenNodes.add(new PathSearchNode(OccupancyGridMap.NortPosition(currentNode.position),currentNode,Constants.NORTH));

				if (map.isSouthFree(currentNode.position))
					childrenNodes.add(new PathSearchNode(OccupancyGridMap.SouthPosition(currentNode.position),currentNode,Constants.SOUTH));

				if (map.isEastFree(currentNode.position))
					childrenNodes.add(new PathSearchNode(OccupancyGridMap.EastPosition(currentNode.position),currentNode,Constants.EAST));

				if (map.isWestFree(currentNode.position))
					childrenNodes.add(new PathSearchNode(OccupancyGridMap.WestPosition(currentNode.position),currentNode,Constants.WEST));
				
				
				for (int i = 0; i < childrenNodes.size(); i++) {
					PathSearchNode child = childrenNodes.get(i);
					if (!openNodes.contains(child) && (!closedNodes.contains(child)))
						openNodes.add(child);
				}
			}
			else
			{
				goalReached = true;
			}
		}

		if (goalReached)
			reconstrucPath(currentNode, path);

		return path;
	}

	private void reconstrucPath(PathSearchNode currentNode, ArrayList<Integer> path )
	{
		if (currentNode.cameFrom != null)
		{
			path.add(0, currentNode.operation);
			reconstrucPath(currentNode.cameFrom, path);
		}
	}

}
